/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#include "USBLSensorGen.hpp"
#include "../ROSInterface.h"
#include "../ConfigXMLParser.h"
#include "../ROSInterface.h"
#include "../USBLSensor.hpp"

using plugin::USBLSensorFactory;

USBLSensorFactory::USBLSensorFactory():config(new XMLUSBLSensor()){config->init();};

USBLSensorFactory::ROSInterfacePtr
USBLSensorFactory::createRosInterface(ros::NodeHandle nh, ROSInterfaceInfo* info)
{
	std::cout<<"Create ROS interface..."<<std::endl;
	ROSInterfacePtr ptr = boost::shared_ptr<USBLSensorToROS>(new USBLSensorToROS(usbl.get(),info->topic, info->rate));
	ptr->nh_ = nh;
	std::cout<<"ROS interface created."<<std::endl;
	return ptr;
}

/**
 * Create the sensor.
 * \todo add return value when it goes to factory.
 */
void USBLSensorFactory::createSensor(plugin::UWSimHook::Ptr hook)
{
	std::cerr<<"Create sensor..."<<std::endl;
	usbl.reset(new USBLSensor(hook->scene, config->name, hook->vMs, hook->scene->localizedWorld->getMatrix(), config->std,config->transponders));
	std::cerr<<"Sensor created."<<std::endl;
}
/**
 * Get the XML configuration ROS node name. Needed for compatibility.
 */
std::string USBLSensorFactory::getRosInterfaceXmlName()
{
	return std::string("USBLSensorGenToROS");
}
/**
 * Get the XML configuration name of the sensor. Needed for compatibility.
 */
std::string USBLSensorFactory::getSensorXmlName()
{
	return std::string("usblSensorGen");
}

/**
 * Get the XML configuration name of the sensor. Needed for compatibility.
 */
std::string USBLSensorFactory::getSensorSceneName()
{
	return config->name;
}

/**
 * Get generic XML format.
 */
plugin::SensorXML::Ptr USBLSensorFactory::getGenericSensorXML()
{
	plugin::SensorXML::Ptr ptr(new plugin::SensorXML());
	ptr->link = config->link;
	ptr->linkName = config->linkName;
	ptr->name = config->name;
	memcpy(ptr->orientation,config->orientation,sizeof(ptr->orientation));
	memcpy(ptr->position, config->position, sizeof(ptr->position));

	return ptr;
}

/**
 * Configure the sensor using the XML node.
 */
void USBLSensorFactory::configureSensor(const xmlpp::Node* node)
{
	xmlpp::Node::NodeList list = node->get_children();
		for(xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter){
			xmlpp::Node* child=dynamic_cast<const xmlpp::Node*>(*iter);

			if(child->get_name()=="position")
				ConfigFile::extractPositionOrColor(child,config->position);
			else if(child->get_name()=="relativeTo")
				ConfigFile::extractStringChar(child,config->linkName);
			else if(child->get_name()=="orientation")
				ConfigFile::extractOrientation(child,config->orientation);
			else if(child->get_name()=="name")
				ConfigFile::extractStringChar(child,config->name);
			else if(child->get_name()=="std")
				ConfigFile::extractFloatChar(child,config->std);
			else if(child->get_name() == "transponder")
			{
				///\todo Replace this quick fix solution for reading attributes
				xmlpp::Node::NodeList names = child->get_children("name");
		   	std::string name;
		   	ConfigFile::extractStringChar(names.front(),name);
				xmlpp::Node::NodeList ids = child->get_children("id");
				double id=0;
				ConfigFile::extractFloatChar(ids.front(),id);
				config->transponders.insert(std::make_pair(id,name));
			}
		}
}


